﻿using System;
using System.Collections.Generic;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace Abyss.Communicator
{
    public class GunCommunicator
    {
        #region Data Members

        private const int c_NumOfStepsInMeshTransfer = 5;
        private const int c_WaitingTimeInMeshTransferInMiliseconds = 50;
        private const double c_XRadiusRatioBetweenGunAndServo = 7.4736;
        private const double c_YRadiusRatioBetweenGunAndServo = 3.89473;
        private readonly double r_XMaxAngle = GetXGunAngleFromXServoAngle(180);
        private readonly double r_YMaxAngle = GetYGunAngleFromYServoAngle(180);

        private const int c_CommunicationBaudRate = 9600;
        private SerialPort m_GunPort;
        private double m_XAngle;
        private double m_YAngle;
        private bool m_IsActivated;

        #endregion

        #region Ctor

        public GunCommunicator(string portName)
        {
            m_GunPort = new SerialPort(portName, c_CommunicationBaudRate, Parity.None, 8, StopBits.One);
            m_IsActivated = true;
        }

        #endregion

        #region Properties

        public bool IsActivated
        {
            get
            {
                return m_IsActivated;
            }
            set
            {
                m_IsActivated = false;

                if (m_IsActivated == false)
                {
                    ChangeGunAngle(1, 1);
                }
                else
                {
                    ChangeGunAngle(90, 45);
                }
            }
        }

        #endregion

        #region Public Methods

        public static string[] GetAvailablePorts()
        {
            return SerialPort.GetPortNames();
        }

        public void Fire()
        {
            ChangeGunAngleAndFire(m_XAngle, m_YAngle, true);
        }

        public void ChangeGunAngle(double xAngle, double yAngle)
        {
            ChangeGunAngleAndFire(xAngle, yAngle, false);
        }

        public void ChangeGunAngleAndFire(double xAngle, double yAngle, bool fire)
        {
            if (m_IsActivated)
            {
                xAngle = GetXServoAngleFromXGunAngle(xAngle);
                yAngle = GetYServoAngleFromYGunAngle(yAngle);

                //double[] xMeshAngles = CreateMeshTransfer(m_XAngle, xAngle);
                double[] yMeshAngles = CreateMeshTransfer(m_YAngle, yAngle);

                MoveServoesWithMesh(fire, xAngle, yMeshAngles);

                SaveNewAngles(xAngle, yAngle);
            }
        }

        private void MoveServoesWithMesh(bool fire, double xAngle, double[] yMeshAngles)
        {
            try
            {
                m_GunPort.Open();

                byte xByteAngle = (byte)xAngle;

                for (int i = 0; i < c_NumOfStepsInMeshTransfer; i++)
                {
                    double yCurrentAngle = yMeshAngles[i];

                    byte yByteAngle = (byte)yCurrentAngle;

                    bool isFire = false;

                    if (i == c_NumOfStepsInMeshTransfer - 1)
                    {
                        isFire = fire;
                    }

                    byte fireByte = (isFire) ? (byte)254 : (byte)0;

                    m_GunPort.Write(new byte[] { xByteAngle, yByteAngle, fireByte }, 0, 3);

                    Thread.Sleep(c_WaitingTimeInMeshTransferInMiliseconds);
                }
            }
            finally
            {
                m_GunPort.Close();
            }
        }

        #endregion

        #region Private Methods

        private double[] CreateMeshTransfer(double originalAngle, double endingAngle)
        {
            List<double> angleList = new List<double>();

            double newAngle = originalAngle;



            for (int i = 0; i < c_NumOfStepsInMeshTransfer; i++)
            {
                newAngle = (newAngle + endingAngle) / 2;

                angleList.Add(newAngle);
            }

            return angleList.ToArray();
        }

        private static double GetXGunAngleFromXServoAngle(double xServoAngle)
        {
            return GetGunAngleFromServoAngle(xServoAngle, c_XRadiusRatioBetweenGunAndServo);
        }

        private static double GetYGunAngleFromYServoAngle(double xServoAngle)
        {
            return GetGunAngleFromServoAngle(xServoAngle, c_YRadiusRatioBetweenGunAndServo);
        }

        private static double GetGunAngleFromServoAngle(double servoAngle, double ratioDifferent)
        {
            servoAngle = NormalizeAngle(servoAngle);

            return (servoAngle / ratioDifferent);
        }

        private static double GetXServoAngleFromXGunAngle(double xGunAngle)
        {
            return GetServoAngleFromGunAngle(xGunAngle, c_XRadiusRatioBetweenGunAndServo);
        }

        private static double GetYServoAngleFromYGunAngle(double yGunAngle)
        {
            return GetServoAngleFromGunAngle(yGunAngle, c_YRadiusRatioBetweenGunAndServo);
        }

        private static double GetServoAngleFromGunAngle(double gunAngle, double gunServoRadiusRatio)
        {
            double servoAngle = gunAngle * gunServoRadiusRatio;

            servoAngle = NormalizeAngle(servoAngle);

            return servoAngle;
        }

        private void SaveNewAngles(double xAngle, double yAngle)
        {
            m_XAngle = xAngle;
            m_YAngle = yAngle;
        }

        private static double NormalizeAngle(double angle)
        {
            angle %= 360;

            if (angle > 178)
            {
                angle = 178;
            }
            else if (angle < 2)
            {
                angle = 2;
            }

            return angle;
        }

        #endregion
    }
}
